#include <rov/motor.h>
#include <rov/util.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>

#include <iostream>
#include <sstream>


using namespace std;


#define BAUD_RATE	B57600
#define MOTOR_MSG_CHAR	'M'
#define MOTOR_MSG_SIZE	5
//#define MOTOR_MSG_SIZE	2

#define FIRST_DEVICE	0
#define LAST_DEVICE	6
#define SPEED_STOP	100


static char const * const MOTORS_ID = "motors";
static char const * const DEVICE_PATTERN = "/dev/ttyACM";
static int motorFd = -1;


Sensor initMotors()
{
    int fd = -1;

    while (fd < 0) {
        for (int i = 0; i < 10 && fd < 0; i++)
        {
            stringstream name;

            name << DEVICE_PATTERN << i;

cerr << "checking device: " << name.str() << endl;

            fd = open(name.str().c_str(), O_RDWR | O_NOCTTY);

            if (fd >= 0 && (configSerial(fd, BAUD_RATE)
                            || !checkMatches(fd, MOTORS_ID)))
            {
                close(fd);
                fd = -1;
            }
        }
    }

    if (fd >= 0)
    {
        errno = 0;
        motorFd = fd;
    }

    return fd;
}


// returns 0 on success
int setMotorSpeed(Motor m, Speed s)
{
    if (motorFd < 0 || m < 0)
        return -1;

    char buf[MOTOR_MSG_SIZE];

    //buf[0] = m;
    //buf[1] = s;

    buf[0] = MOTOR_MSG_CHAR;
    buf[1] = m;
    buf[2] = s;
    buf[3] = m;
    buf[4] = s;

cerr << "sending cmd (" << MOTOR_MSG_SIZE << "): " << (int) buf[1]
     << " " << (int) buf[2] << endl;

    unsigned int n = 0;

    while (n < MOTOR_MSG_SIZE)
    {
        int amt = write(motorFd, &buf[n], MOTOR_MSG_SIZE - n);
        //int amt = write(motorFd, &buf[n], 1);

        if (amt < 0)
            break;

cerr << "sent: " << amt << endl;

        // should be at least 10000
//        usleep(15000);

        n += amt;
    }

cerr << "sent total: " << n << endl;

//while (n != MOTOR_MSG_SIZE);

    return n != MOTOR_MSG_SIZE;
}


void stopMotors()
{
    for (int i = FIRST_DEVICE; i <= LAST_DEVICE; i++)
        setMotorSpeed(i, SPEED_STOP);
}
